#include "compass.h"


#define min(a,b) ((a<b)?a:b)
#define max(a,b) ((a>b)?a:b)

Compass_Data_t Compass_Data;


float Compass_x_Data[Fitting_NUM];
float Compass_y_Data[Fitting_NUM];
float Compass_z_Data[Fitting_NUM];

uint8_t i,Fitting_Flag,Ellipsoid_fitting_Flag,Level_Flag;
uint16_t Horizontal;

#if COMPASS_TEST
int Show_Horizontal_Measurement_Angle_t;
int Show_Horizontal_Measurement_Angle_Filter_t;
#endif

void Compass_Data_Init(Compass_Data_t *Compass_Data)
{
  Compass_Data->Level_Data.X_Min = 500;
	Compass_Data->Level_Data.Y_Min = 500;
}

void Compass_Init_Filter(Compass_Data_t *Compass_Data)
{
 SmoothFilter_Init(&Compass_Data->SmoothFilter, FILTER_NUM);
}

void Compass_Data_Processing(Compass_Data_t *Compass_Data)
{
	
	mpu_get_accel_reg(Compass_Data->Base_Data.Accel_Data,NULL);
	Compass_Data->Base_Data.Accel_x = (float)Compass_Data->Base_Data.Accel_Data[0] / 1671.837f;
	Compass_Data->Base_Data.Accel_y = (float)Compass_Data->Base_Data.Accel_Data[1] / 1671.837f;
	Compass_Data->Base_Data.Accel_z = (float)Compass_Data->Base_Data.Accel_Data[2] / 1671.837f;
	
	
	if(fabs(Compass_Data->Base_Data.Accel_x) < 0.5 && fabs(Compass_Data->Base_Data.Accel_y) < 0.5 || Level_Flag == 1)
	{
	Level_Flag = 1;
	mpu_dmp_get_data(&Compass_Data->IMU_Pitch, &Compass_Data->IMU_Roll, &Compass_Data->IMU_Yaw);
	
	}

	//8字旋转 椭球拟合校准
	if(fabs(Compass_Data->Base_Data.Accel_x) > 0.5 && fabs(Compass_Data->Base_Data.Accel_y) > 0.5)
	{
		if( (!mpu_get_compass_reg(Compass_Data->Base_Data.Compass_Data,NULL)) && i < Fitting_NUM)
		{
			
			Compass_Data->Base_Data.Compass_x = (float)Compass_Data->Base_Data.Compass_Data[0];
			Compass_Data->Base_Data.Compass_y = (float)Compass_Data->Base_Data.Compass_Data[1];
			Compass_Data->Base_Data.Compass_z = (float)Compass_Data->Base_Data.Compass_Data[2];
		
			Compass_x_Data[i] = Compass_Data->Base_Data.Compass_x;
			Compass_y_Data[i] = Compass_Data->Base_Data.Compass_y;
			Compass_z_Data[i] = Compass_Data->Base_Data.Compass_z;	
			i++;
		  Ellipsoid_fitting_Flag = 1;
		}
		Fitting_Flag = 1;
  }
	
	if(Ellipsoid_fitting_Flag == 1 && i == Fitting_NUM)
	{
		//椭球拟合校准  (程序校准之初调用，此后不在使用)
		Ellipsoid_fitting_Process(Compass_x_Data, Compass_y_Data, Compass_z_Data);
	  Ellipsoid_fitting_Flag = 0;
		Horizontal = 0;
	}
	
	//椭球拟合完毕 读取磁力计数据并进行处理
	if(Fitting_Flag == 1)
	{
		
	//mpu_get_compass_reg(Compass_Data->Base_Data.Compass_Data, 100);
	
	Compass_Data->Base_Data.X_uT = Compass_Data->Base_Data.Compass_Data[0] ;
	Compass_Data->Base_Data.Y_uT = Compass_Data->Base_Data.Compass_Data[1] ;
	Compass_Data->Base_Data.Z_uT = Compass_Data->Base_Data.Compass_Data[2] ;
	
		if(Compass_Data->Ellipsoid_fitting_Data->Data_A != 0 && Compass_Data->Ellipsoid_fitting_Data->Data_C != 0 && Compass_Data->Ellipsoid_fitting_Data->Data_B != 0)
		{
			Compass_Data->Base_Data.X_uT = (Compass_Data->Base_Data.X_uT - Compass_Data->Ellipsoid_fitting_Data->Data_X0) / Compass_Data->Ellipsoid_fitting_Data->Data_A;
			Compass_Data->Base_Data.Y_uT = (Compass_Data->Base_Data.Y_uT - Compass_Data->Ellipsoid_fitting_Data->Data_Y0) / Compass_Data->Ellipsoid_fitting_Data->Data_B;
			Compass_Data->Base_Data.Z_uT = (Compass_Data->Base_Data.Z_uT - Compass_Data->Ellipsoid_fitting_Data->Data_Z0) / Compass_Data->Ellipsoid_fitting_Data->Data_C;
		
		}
	}
	
	//水平测量
	if(fabs(Compass_Data->Base_Data.Accel_x) < 0.5 && fabs(Compass_Data->Base_Data.Accel_y) < 0.5)
	{
		
		Compass_Data->Level_Data.X_Max = max(Compass_Data->Level_Data.X_Max , Compass_Data->Base_Data.X_uT);
		Compass_Data->Level_Data.Y_Max = max(Compass_Data->Level_Data.Y_Max , Compass_Data->Base_Data.Y_uT);

		Compass_Data->Level_Data.X_Min = min(Compass_Data->Level_Data.X_Min , Compass_Data->Base_Data.X_uT);
		Compass_Data->Level_Data.Y_Min = min(Compass_Data->Level_Data.Y_Min , Compass_Data->Base_Data.Y_uT);
		

	  Compass_Data->Level_Data.X_Offset = (Compass_Data->Level_Data.X_Max + Compass_Data->Level_Data.X_Min)/2;
		Compass_Data->Level_Data.Y_Offset = (Compass_Data->Level_Data.Y_Max + Compass_Data->Level_Data.Y_Min)/2;
		
		Compass_Data->X_H = Compass_Data->Base_Data.X_uT - Compass_Data->Level_Data.X_Offset;
		Compass_Data->Y_H = Compass_Data->Base_Data.Y_uT - Compass_Data->Level_Data.Y_Offset;
		 

		if(Compass_Data->X_H < 0 && Compass_Data->Y_H > 0)
		 Compass_Data->Compass_Angle =   360 - (atan2(Compass_Data->Y_H , Compass_Data->X_H) * 180 /3.14159);		 

		else if(Compass_Data->X_H < 0 && Compass_Data->Y_H < 0)
		 Compass_Data->Compass_Angle =  - (atan2(Compass_Data->Y_H , Compass_Data->X_H) * 180 /3.14159);		 

		else if(Compass_Data->X_H > 0 && Compass_Data->Y_H < 0)
		 Compass_Data->Compass_Angle = - (atan2(Compass_Data->Y_H , Compass_Data->X_H) * 180 /3.14159);

		else if(Compass_Data->X_H > 0 && Compass_Data->Y_H > 0)
		 Compass_Data->Compass_Angle = 360 - (atan2(Compass_Data->Y_H , Compass_Data->X_H) * 180 /3.14159);

		else if(Compass_Data->X_H == 0 && Compass_Data->Y_H < 0)
		 Compass_Data->Compass_Angle = 90;

		else if(Compass_Data->X_H == 0 && Compass_Data->Y_H > 0)
		 Compass_Data->Compass_Angle = 270;
		
	if(Compass_Data->Compass_Angle < 5 || Compass_Data->Compass_Angle > 355)
		{
			if(Compass_Data->Compass_Angle < 1 || Compass_Data->Compass_Angle > 359)
			  Compass_Data->Compass_Angle_Filter = 0;
			
			Compass_Data->Compass_Angle_Filter = Compass_Data->Compass_Angle ;
		}
		
		else
			Compass_Data->Compass_Angle_Filter = SmoothFilter_Calc(&Compass_Data->SmoothFilter, Compass_Data->Compass_Angle);
			 
#if COMPASS_TEST		
		Show_Horizontal_Measurement_Angle_t = Compass_Data->Horizontal_Measurement_Angle * 100;
		Show_Horizontal_Measurement_Angle_Filter_t = Compass_Data->Horizontal_Measurement_Angle_Filter * 100;
		SEGGER_RTT_Write(1,&Show_Horizontal_Measurement_Angle_t, sizeof(Show_Horizontal_Measurement_Angle_t));
		SEGGER_RTT_Write(1,&Show_Horizontal_Measurement_Angle_Filter_t, sizeof(Show_Horizontal_Measurement_Angle_Filter_t));
#endif
	}
	else
	{
		
	}				
}

void Ellipsoid_fitting_Init(Compass_Data_t *Compass_Data)
{
 Compass_Data->Ellipsoid_fitting_Data = get_Ellipsoid_fitting_Data();
}

